function [param,gp] = gpSARSA(param,gp,r,u,n,t)
%
% gpSARSA(model,gp,xtmin1,xt,rtmin1,utmin1,ut,t)
% xtmin1: state at t-1
% xt: state at t
%
% GPDARSA implementation based on Y. Engel's 2005 dissertation (sparse
% recursive nonparameteric Monte-Carlo GPSARSA algo).
%
% Note: all timeseries start with t==0 in the first entry, 
% i.e. var(1) is the value of var at time 0.
%
% Tobias Siegfried, 06/10/2008

% observe states, actions and rewards


if t == 1
    xtmin1 = param.stateV{n}(t,:)';
    rtmin1 = 0; utmin1 = 0;
    xt = param.stateV{n}(t,:)'; ut = u(n,t);
    gp.dictS0 = [xtmin1; utmin1];
    xDictT = gp.dictS0(1:param.stateVDim,1);
    xDictTmin1 = xDictT;
    gp.KTildeInv0 = 1 / (gaussK(xtmin1,xtmin1,param.kSigmaS) .* linK(xtmin1,xtmin1,param.kSigmaA));
    gp.cTilde0 = 0; % ok
    gp.d0 = 0; % ok
    gp.dictS{n}
else
    coeff = param.gamma(n) * param.sigma^2 / gp.s{n}(t-1);
    xtmin1 = param.stateV{n}(t-1,:)'; 
    rtmin1 = r(n,t-1); 
    utmin1 = u(n,t-1);
    xt = param.stateV{n}(t,:)'; 
    ut = u(n,t);
    xDictT = gp.dictS{n}(1:gp.latestDictSI{n},1:param.stateVDim)';
    if param.dictChange{n}
        xDictTmin1 = gp.dictS{n}(1:gp.latestDictSI{n}-1,1:param.stateVDim)';
    else
        xDictTmin1 = xDict;
    end
end
kTildeXtmin1 = gaussK(xDictTmin1,xtmin1,param.kSigmaS) * linK(xDictTmin1,xtmin1,param.kSigmaA);
kTildeXt = gaussK(xDictTmin1,xt,param.kSigmaS) * linK(xDictTmin1,xt,param.kSigmaA);
gp.dkTilde{t,n} = kTildeXtmin1 - param.gamma(n) * kTildeXt; % ok
if t == 1
    gp.delta{n}(t) = gaussK(xt,xt,param.kSigmaS) * linK(xt,xt,param.kSigmaA);
    gp.d{n}(t) = - rtmin1; % ok
    gp.CTilde{t,n} = 0; % ok
    gp.alphaTilde{t,n} = 0; % ok
else
    gp.delta{n}(t) = gaussK(xt,xt,param.kSigmaS) * linK(xt,xt,param.kSigmaA) - gp.a{t,n}' * kTildeXt;
    gp.d{n}(t) = coeff * gp.d{n}(t-1) - rtmin1 + gp.dkTilde{t,n}' * gp.alphaTilde{t-1,n}; % ok
end
if gp.delta{n}(t) > param.sparseP % add to dictionary
    gp.a{t,n} = [zeros(length(gp.a{t,n})-1,1); 1];
    if t == 1
        dktt = param.gamma(n)^2 * gaussK(xt,xt,param.kSigmaS) * linK(xt,xt,param.kSigmaA); % ok
        aHat = 0;
        gp.KTildeInv{n,t} = 1 / gp.delta{n}(t) * [[gp.KTildeInv0; aHat'] [aHat; 1]]; % ok
        gp.cTilde{t,n} = 0; % ok
        gp.s{n}(t) = param.sigma^2 * (1 + param.gamma(n)^2) + dktt - gp.dkTilde{t,n}' * gp.cTilde{t,n}; % ok
        gp.cTilde{t,n} = [0;param.gamma(n)]; % ok
        gp.alphaTilde{t,n} = zeros(2,1); % ok
        gp.CTilde{t,n} = zeros(2); % ok
    else
        dktt = gp.a{t-1,n}' * (kTildeXtmin1 - 2 * param.gamma(n) * kTildeXt) + ...
            param.gamma(n)^2 * gaussK(xt,xt,param.kSigmaS) * linK(xt,xt,param.kSigmaA); % ok
        aHat = gp.KTildeInv{n,t-1} * kTildeXt; % ok
        gp.KTildeInv{n,t} = 1 / gp.delta{n}(t) * [[gp.KTildeInv0; aHat'] [aHat; 1]]; % ok
        gp.cTilde{t,n} = [coeff * gp.cTilde{t-1,n} + gp.CTilde{t-1,n} * gp.dkTilde{t,n} - gp.a{t,n}; param.gamma(n)]; % ok
        
        gp.s{n}(t) = param.sigma^2 * (1 + param.gamma(n)^2) + dktt - coeff * ...
            param.gamma(n) * param.sigma^2 - gp.dkTilde{t,n}' * ...
            (gp.cTilde{t,n} + coeff * gp.cTilde{t-1,n} + gp.a{t-1,n}); % ok
        
        gp.alphaTilde{t,n} = [gp.alphaTilde{t-1,n}; 0] + gp.d{n}(t) / gp.s{n}(t) * gp.cTilde{n,t}; %ok
        
        gp.CTilde{t,n} = [[gp.CTilde{t-1,n} zeros(size(gp.CTilde{t-1,n},1),1)];...
            [zeros(1,size(gp.CTilde{t-1,n},1)) 0]];  
    end
    param.dictChange{n} = 1;
else % no change in dictionary
    dktt = gp.hTilde{t,n} * gp.dkTilde{t,n}; % ok
    if t == 1
        gp.KTildeInv{n,t} = 0; % ok
        gp.a{t,n} = 0; % ok
        gp.hTilde{t,n} = - param.gamma(n)*gp.a{t,n}; % ok
        gp.cTilde{t,n} = - gp.hTilde{t,n}; % ok
        gp.alphaTilde{t,n} = gp.d{n}(t) / gp.s{n}(t) * gp.cTilde{t,n}; % ok
        gp.CTilde{t,n} = 0; % ok
    else
        gp.KTildeInv{n,t} = gp.KTildeInv{n,t-1}; % ok
        gp.a{t,n} = gp.KTildeInv{n,t-1} * kTildeXt; % ok
        gp.hTilde{t,n} = gp.a{t-1,n} - param.gamma(n)*gp.a{t,n}; % ok
        gp.cTilde{t,n} = coeff * gp.cTilde{t-1,n} - gp.hTilde{t,n} + gp.CTilde{t-1,n} * gp.dkTilde{t,n}; % ok
        gp.alphaTilde{t,n} = gp.alphaTilde{t-1,n} + gp.d{n}(t) / gp.s{n}(t) * gp.cTilde{t,n}; % ok
        gp.CTilde{t,n} = gp.CTilde{t-1,n} + 1/gp.s{n}(t)*gp.cTilde{t,n}*gp.cTilde{t,n}^-1;
    end
    param.dictChange{n} = 0;
end

%keyboard










